| Note: This tutorial assumes that you have completed the previous tutorials: Configuring a kinematics plugin. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Specifying pose goals
Description: Specifying pose goalsTutorial Level: INTERMEDIATE
Contents
In this tutorial, you will learn how to send a pose goal for a planner for a manipulator configured with a kinematics plugin (see Configuring a kinematics plugin). We will use the PR2 robot as an example since its already configured in ROS diamondback.
Calling the motion planner
You can call the motion planner by using the GetMotionPlan service request. Here's code for filling in the request and calling the service (which is named ompl_planning/plan_kinematic_path). More details about the fields of the request can be found in the move_arm tutorials.
Save this code in a file with the name ompl_pose_goal.cpp in the ROS package named example_ompl_tutorials you created earlier in this tutorial.
1 #include <ros/ros.h>
2 #include <motion_planning_msgs/GetMotionPlan.h>
3
4 #include <motion_planning_msgs/DisplayTrajectory.h>
5 #include <planning_environment/monitors/joint_state_monitor.h>
6 #include <boost/thread.hpp>
7
8 void spinThread()
9 {
10 ros::spin();
11 }
12
13 int main(int argc, char **argv)
14 {
15 ros::init (argc, argv, "ompl_pose_goal");
16 boost::thread spin_thread(&spinThread);
17
18 ros::NodeHandle nh;
19
20 motion_planning_msgs::GetMotionPlan::Request request;
21 motion_planning_msgs::GetMotionPlan::Response response;
22
23 request.motion_plan_request.group_name = "right_arm";
24 request.motion_plan_request.num_planning_attempts = 1;
25 request.motion_plan_request.planner_id = std::string("");
26 request.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
27
28 request.motion_plan_request.goal_constraints.position_constraints.resize(1);
29 request.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
30 request.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
31
32 request.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
33 request.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75;
34 request.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188;
35 request.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
36
37 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
38 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
39 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
40 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
41
42 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
43 request.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
44
45 request.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
46 request.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
47 request.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
48 request.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
49 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
50 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
51 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
52 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
53
54 request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
55 request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
56 request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
57
58 request.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
59
60
61 ros::ServiceClient service_client = nh.serviceClient<motion_planning_msgs::GetMotionPlan>("ompl_planning/plan_kinematic_path");
62 service_client.call(request,response);
63 if(response.error_code.val != response.error_code.SUCCESS)
64 {
65 ROS_ERROR("Motion planning failed");
66 }
67 else
68 {
69 ROS_INFO("Motion planning succeeded");
70 }
71
72
73 planning_environment::JointStateMonitor joint_state_monitor;
74 ros::Publisher display_trajectory_publisher = nh.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1);
75 while(display_trajectory_publisher.getNumSubscribers() < 1 && nh.ok())
76 {
77 ROS_INFO("Waiting for subscriber");
78 ros::Duration(0.1).sleep();
79 }
80 motion_planning_msgs::DisplayTrajectory display_trajectory;
81
82 display_trajectory.model_id = "pr2";
83 display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint";
84 display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now();
85 display_trajectory.robot_state.joint_state = joint_state_monitor.getJointStateRealJoints();
86 display_trajectory.trajectory = response.trajectory;
87 ROS_INFO("Publishing path for display");
88 display_trajectory_publisher.publish(display_trajectory);
89 joint_state_monitor.stop();
90 ros::shutdown();
91 spin_thread.join();
92 return(0);
93
94 }
Compile it
Add the following line to your CMakelists.txt
rosbuild_add_executable(ompl_pose_goal src/ompl_pose_goal.cpp)
Compile your code:
rosmake example_ompl_tutorials
Run it!
Start the simulator
roslaunch pr2_gazebo pr2_empty_world.launch
Start the motion planner
export ROBOT=sim roslaunch pr2_3dnav right_arm_navigation.launch
Setup rviz for visualizing the plan
Follow the instructions at the bottom of this tutorial (from Point 7. Rviz Setup onwards). You will be setting up a planning model visualization that you can use to visualize the path generated by OMPL.
Run your request
rosrun example_ompl_tutorials ompl_pose_goal
You should be able to see the path visualized in rviz. Play around with the request a bit to see what more you can do!
Specifying multiple goals (Optional)
You could, if you wanted, specify multiple pose goals to the planner. You can do this by adding more position and orientation constraints to the request. The planner will plan to one of the goals in your list. This could be useful if, for example, you want to plan to one of the multiple grasps available for grasping an object.
